/*
*Copyright (c) Nikhil Jain.
*Project ICARUS, communications and 
*artificial intelligence.
*/



/*
* To change this template, choose Tools | Templates
* and open the template in the editor.
 */
package ai.rrt;

//~--- JDK imports ------------------------------------------------------------

import java.util.ArrayList;
import java.util.Iterator;
import java.util.TreeMap;

import javax.vecmath.Vector3d;

/**
 *
 * @author Nikzzay
 */
public class RapidlyExplorableRandomTree {
    public static double distanceBetween(Vector3d a, Vector3d b) {
        Vector3d buff = new Vector3d(b);

        buff.negate();
        buff.add(a);

        return buff.length();
    }

    public static double distanceBetween(Node a, Node b) {
        return distanceBetween(a.coords, b.coords);
    }

    public static boolean isGoalNode(Node candidate, Vector3d goal, double thresh) {
        return distanceBetween(candidate.coords, goal) <= thresh;
    }

    public static ArrayList<Node> getPath(Tree start, Tree end) {
        ArrayList<Node> path = new ArrayList<>();

        path.add(end.value);

        Tree parent = end.parent;

        while (parent != null) {
            if (parent.equals(start)) {
                return path;
            }

            path.add(parent.value);
        }

        return null;
    }

    public static double getPathLength(Node n, Node goal) {
        return n.pathLengthFromRoot + distanceBetween(n, goal);
    }

    public static ArrayList<Node> getNearestNodes(Tree T, Node N, double distance) {
        TreeMap<Double, Node> orderedNodes = new TreeMap<>();
        Iterator              treeIterator = T.nodeIterator();

        while (treeIterator.hasNext()) {
            Node   next = (Node) treeIterator.next();
            double dist = distanceBetween(N, next);

            if (dist <= distance) {
                orderedNodes.put(dist, next);
            }
        }

        ArrayList<Node> nearestNodes = new ArrayList<>(orderedNodes.size());

        for (Node node : orderedNodes.values()) {
            nearestNodes.add(node);
        }

        return nearestNodes;
    }

    public Node getNearestNode(Tree T, Node N) {
        double   min          = Double.MAX_VALUE;
        Node     nearestNode  = null;
        Iterator treeIterator = T.nodeIterator();

        while (treeIterator.hasNext()) {
            Node   next = (Node) treeIterator.next();
            double dist = distanceBetween(N, next);

            if (dist < min) {
                nearestNode = next;
            } else if (dist == min) {
                if (next.cost < nearestNode.cost) {
                    nearestNode = next;
                }
            }
        }

        return nearestNode;
    }
}


//~ Formatted by Jindent --- http://www.jindent.com
